/*
 * @Author: 0x9DEFA478
 * @Date: 2022-08-06 18:21:01
 * @LastEditTime: 2022-08-17 20:10:18
 * @LastEditors: 0x9DEFA478
 * @Description: 
 * QQ:2652450237
 * ============================================================================================================================================
 * 
 * 
 * 
 *                                                                                               ************      ****************************
 *                                                                                             ************      ****************************  
 *                                                                                           ************      ****************************    
 *                                                                                         ************      ****************************      
 *                                                                                       ************      ************                        
 *                                                                                     ************      ************                          
 *                                                                                   ************      ************                            
 *                                                                                 ************      ************                              
 *                                                                               ************      ************                                
 *                                                                             ************      ************                                  
 *                                                                           ************      ************                                    
 *                                                                         ************      ************                                      
 *                                                                       ************      ************                                        
 *                                                                     ************      ************                                          
 *                                                                   ************      ************                                            
 *                                                                 ************      ************                                              
 *                                                               ************      ************                                                
 *                                                             ************      ************                                                  
 *                                                           ************      ************                                                    
 *                                                         ************      ************                                                      
 *                                                       ************      ************                                                        
 *                                                     ************      ************                                                          
 *                                                   ************      ************                                                            
 *                                                 ************      ************                                                              
 *                                               ************      ************                                                                
 *                                             ************      ************                                                                  
 *                                           ************      ************                                                                    
 *                                         ************      ************                                                                      
 *                                       ************      ************                                                                        
 *                                     ************      ************                                                                          
 *                                   ************      ************                                                                            
 *                                 ************      ************                                                                              
 *                               ************      ************                                                                                
 *                             ************      ************                                                                                  
 *                           ************      ************                                                                                    
 *                         ************      ************                                                                                      
 *       ****************************      ************                                                                                        
 *     ****************************      ************                                                                                          
 *   ****************************      ************                                                                                            
 * ****************************      ************                                                                                              
 * 
 * 
 * 
 * ============================================================================================================================================
 * 
 */
#include "H_NES_CPP.h"
#include "ByteStream.h"
#include <malloc.h>
#include <windows.h>
#include <mutex>
#include <iostream>

extern "C" {
#include "H_NES.h"
#include "H_AudioResample.h"
}




#define vNES_Audio_LPF_Fc                                         16000  //低通滤波器截止频率
#define vNES_Audio_HPF_Fc                                         1      //高通滤波器截止频率

#define cNES_Audio_FloatConst(val)                                (val##f)




#define vAudioOutput_InputMaxSampleRate                           50000 //输入最大采样率
#define vAudioOutput_OutputMaxSampleRate                          384000 //输出最大采样率




#define vAudioOutput_SizeOfPerOutput                              (2*vAudioOutput_OutputMaxSampleRate/1000) //每个输出包的最大大小: 声道数*最大采样率/最大采样率下的理想包输出频率
#define vAudioOutput_BufferBlockAllCount                          8
#define vAudioOutput_BufferSize                                   (vAudioOutput_BufferBlockAllCount*vAudioOutput_SizeOfPerOutput)
#define vAudioOutput_InputBufferSize                              768 //输入缓存使用大小 单位:sizeof(H_AudioResample_FloatType)字节




#define vAudioConvertOutput_MaxNumOfSample                        256




typedef struct{
  H_NES_CPP* nes_cpp;
  void* nes;

  struct{
    
    H_AudioResample_InputType* AudioConvertOutputBuffer;

    struct{
      H_AudioResample_InputType K;
      H_AudioResample_InputType LY1;
      H_AudioResample_InputType RY1;
    }LPF;
    struct{
      H_AudioResample_InputType K;
      H_AudioResample_InputType LY1;
      H_AudioResample_InputType LX1;
      H_AudioResample_InputType RY1;
      H_AudioResample_InputType RX1;
    }HPF;
    H_AudioResample_InputType PulseLR_Diff;//左右声道方波占比

    H_AudioResample* Resampler;
    H_AudioResample_FloatType* InputBuffer;
    H_AudioResample_OutputType* OutputBuffer;
    
    struct{
      struct{
        int Channel[3];
      }Ctrl;
    }Volume;

    int InSampleRate;
    int OutSampleRate;
    int SizeOfPerOutput;

    
    std::recursive_mutex* Lock;
  }Audio;

  HANDLE NES_ThreadHandle;

  unsigned char* ColorIndexArray;

  struct{
    bool IsRun;
    std::recursive_mutex* Lock;
  }Status;
  
  Huint16 LastStandardControllerStatus;

}H_NES_C;




static void SetHPFLPF(H_NES_C* nes_c,double LPFreq,double HPFreq){
  H_AudioResample_InputType K;

  K=(H_AudioResample_InputType)(1.0/(1.0+LPFreq/(2.0*3.141592653589793*vNES_Audio_LPF_Fc)));
  nes_c->Audio.LPF.K=K;
  nes_c->Audio.LPF.LY1=cNES_Audio_FloatConst(0.0);
  nes_c->Audio.LPF.RY1=cNES_Audio_FloatConst(0.0);

  K=(H_AudioResample_InputType)((HPFreq/(2.0*3.141592653589793*vNES_Audio_HPF_Fc))/(1.0+(HPFreq/(2.0*3.141592653589793*vNES_Audio_HPF_Fc))));
  nes_c->Audio.HPF.K=K;
  nes_c->Audio.HPF.LY1=cNES_Audio_FloatConst(0.0);
  nes_c->Audio.HPF.RY1=cNES_Audio_FloatConst(0.0);
  nes_c->Audio.HPF.LX1=cNES_Audio_FloatConst(0.0);
  nes_c->Audio.HPF.RX1=cNES_Audio_FloatConst(0.0);

}

void NES_SetPulseLR_Diff(H_NES_C* nes_c,int PulseLR_Diff){

  if(PulseLR_Diff==0){
    nes_c->Audio.PulseLR_Diff=cNES_Audio_FloatConst(0.0);
    return;
  }
  if(PulseLR_Diff<0){
    PulseLR_Diff=-PulseLR_Diff;
    if(PulseLR_Diff>100){
      PulseLR_Diff=100;
    }

    nes_c->Audio.PulseLR_Diff=-(cNES_Audio_FloatConst(1.0)-H_AudioResample_GetMultiple(100-PulseLR_Diff));
  }else{
    if(PulseLR_Diff>100){
      PulseLR_Diff=100;
    }

    nes_c->Audio.PulseLR_Diff=cNES_Audio_FloatConst(1.0)-H_AudioResample_GetMultiple(100-PulseLR_Diff);
  }

}

static void SetVolume(H_NES_C* nes_c){

  H_AudioResample_FloatType MasterMultiple;
  H_AudioResample_FloatType VolumeMultiples[2];

  MasterMultiple=H_AudioResample_GetMultiple(nes_c->Audio.Volume.Ctrl.Channel[0]);
  VolumeMultiples[0]=H_AudioResample_GetMultiple(nes_c->Audio.Volume.Ctrl.Channel[1]);
  VolumeMultiples[1]=H_AudioResample_GetMultiple(nes_c->Audio.Volume.Ctrl.Channel[2]);

  H_AudioResample_SetVolumeMultiple(nes_c->Audio.Resampler,MasterMultiple,VolumeMultiples);

}

static void SetSampleRate(H_NES_C* nes_c){
  int Div;
  
  H_AudioResample_SetSuspend(nes_c->Audio.Resampler,-1);

  H_AudioResample_SetIONumOfSample(nes_c->Audio.Resampler,nes_c->Audio.InSampleRate,nes_c->Audio.OutSampleRate);

  switch(nes_c->Audio.OutSampleRate){
    case 44100:
    case 48000:
      Div=8;
      break;
    case 88200:
    case 96000:
      Div=4;
      break;
    case 176400:
    case 192000:
      Div=2;
      break;
    case 352800:
    case 384000:
    default:
      Div=1;
      break;
  }

  nes_c->Audio.SizeOfPerOutput=vAudioOutput_SizeOfPerOutput/Div;
  H_AudioResample_SetSizeOfPerOutput(nes_c->Audio.Resampler,nes_c->Audio.SizeOfPerOutput);

  H_AudioResample_SetSuspend(nes_c->Audio.Resampler,0);

}

static const char* H_NES_GetErrorString(int Error){
  switch(Error){
    case vH_NES_Result_ROMNotLoad:
      return "ROM未加载";
    case vH_NES_Result_ROMLoaded:
      return "ROM已经加载";
    case vH_NES_Result_Running:
      return "H_NES正在运行";
    case vH_NES_Result_Stopped:
      return "H_NES已经停止";
    case vH_NES_Result_ROMError:
      return "不能识别的ROM";
    case vH_NES_Result_MapperNoSupport:
      return "不支持的Mapper";
    case vH_NES_Result_PRG_RAM_AllocFail:
      return "PRG_RAM分配失败";
    case vH_NES_Result_CHR_RAM_AllocFail:
      return "CHR_RAM分配失败";
    case vH_NES_Result_FetchInstructionFail:
      return "意外的取指访问";
    case vH_NES_Result_PRG_ReadFail:
      return "意外的PRG读访问";
    case vH_NES_Result_PRG_WriteFail:
      return "意外的PRG写访问";
    case vH_NES_Result_PPUError:
      return "PPU错误";
    case vH_NES_Result_APUError:
      return "APU错误";
    case vH_NES_Result_ParamError:
      return "参数错误";
    case vH_NES_Result_UnknowError:
    default:
      return "未知错误";
  }
}




//用于NES的内存分配函数
static void* CALLBACK Malloc(Husize Size){
  return malloc(Size);
}

//用于NES的内存释放函数
static void CALLBACK Free(void* Ptr){
  free(Ptr);
}

//NES退出前会调用
static void CALLBACK ExitCallback(void* v){
  H_NES_C* handle;

  handle=(H_NES_C*)v;
  
  handle->Status.Lock->lock();
  handle->Status.IsRun=false;
  handle->Status.Lock->unlock();

}

static DWORD CALLBACK NES_Thread(LPVOID v){
  H_NES_C* handle;
  int r;

  handle=(H_NES_C*)v;
  
  handle->Status.Lock->lock();
  handle->Status.IsRun=true;
  handle->Status.Lock->unlock();

  r=vH_NES_Result_UnknowError;

  //Release版本下, 调用C函数时, 其内部抛异常会导致程序crash
  //使用
//  try{
    H_NES_Start(handle->nes);
//  }catch(int e){
//    r=e;
//  }catch(...){
//    //不应该捕获到这种异常
//    std::cout<<"H_NES:UnknowException!"<<std::endl;
//  }

  return (DWORD)r;
}

static void GetLL_Function(H_NES_LL_Function* LL_Function);




H_NES_CPP::H_NES_CPP(SampleRate AudioSampleRate):tvStandards(TVStandards::NTSC){
  H_NES_C* handle;
  H_NES_LL_Function LL_Function;
  int i;
  
  handle=new H_NES_C();

  handle->nes_cpp=this;




  //音频相关

  handle->Audio.Volume.Ctrl.Channel[0]=67;
  handle->Audio.Volume.Ctrl.Channel[1]=100;
  handle->Audio.Volume.Ctrl.Channel[2]=100;
  handle->Audio.InSampleRate=H_NES_CPP::GetCPUFreq(tvStandards)/4;
  handle->Audio.OutSampleRate=(int)AudioSampleRate;

  handle->Audio.AudioConvertOutputBuffer=new H_AudioResample_InputType[2*vAudioConvertOutput_MaxNumOfSample];
  handle->Audio.Resampler=new H_AudioResample();
  handle->Audio.InputBuffer=new H_AudioResample_FloatType[vAudioOutput_InputBufferSize];
  handle->Audio.OutputBuffer=new H_AudioResample_OutputType[vAudioOutput_BufferSize];
  handle->Audio.Lock=new std::recursive_mutex();

  H_AudioResample_Init(handle->Audio.Resampler
    ,handle->Audio.OutputBuffer,vAudioOutput_BufferBlockAllCount,vAudioOutput_SizeOfPerOutput
    ,handle->Audio.InputBuffer,vAudioOutput_InputBufferSize);
  H_AudioResample_SetResample(handle->Audio.Resampler,cH_AudioResample_FloatConst(0.0002)
    ,handle->Audio.InSampleRate,handle->Audio.OutSampleRate
    ,0,0,0,0);
  H_AudioResample_SetOutputThreshold(handle->Audio.Resampler,2);
  SetVolume(handle);
  H_AudioResample_SetIsDefaultSpeed(handle->Audio.Resampler,-1);
  H_AudioResample_SetBufferBlockMaxCount(handle->Audio.Resampler,vAudioOutput_BufferBlockAllCount-2);

  H_AudioResample_SetSuspend(handle->Audio.Resampler,0);
  SetSampleRate(handle);
  SetHPFLPF(handle,handle->Audio.InSampleRate,handle->Audio.InSampleRate);
  NES_SetPulseLR_Diff(handle,0);


  handle->NES_ThreadHandle=nullptr;

  //视频相关

  handle->ColorIndexArray=new unsigned char[256*240];

  for(i=0;i<256*240;i++){
    handle->ColorIndexArray[i]=0x3FU;
  }




  //NES


  GetLL_Function(&LL_Function);

  LL_Function.v=handle;
  LL_Function.ExitCallback=ExitCallback;

  handle->nes=new_H_NES_Console(&LL_Function,Malloc,Free);




  //其他

  handle->Status.IsRun=false;
  handle->Status.Lock=new std::recursive_mutex();




  handle->LastStandardControllerStatus=0U;




  this->handle=handle;
}

H_NES_CPP::~H_NES_CPP(){
  H_NES_C* handle;

  handle=(H_NES_C*)this->handle;

  if(0!=H_NES_IsRun(handle->nes)){
    Stop();
  }

  delete handle->Status.Lock;

  delete_H_NES_Console(handle->nes);

  delete handle->Audio.Resampler;
  delete[] handle->Audio.InputBuffer;
  delete[] handle->Audio.OutputBuffer;
  delete[] handle->Audio.AudioConvertOutputBuffer;
  delete handle->Audio.Lock;

  delete[] handle->ColorIndexArray;


  delete handle;

}

void H_NES_CPP::SetOutputSampleRate(SampleRate sampleRate){
  H_NES_C* handle;

  handle=(H_NES_C*)this->handle;

  handle->Audio.Lock->lock();

  handle->Audio.OutSampleRate=(int)sampleRate;
  SetSampleRate(handle);

  handle->Audio.Lock->unlock();
}

void H_NES_CPP::SetVolume_M(int Volume)noexcept{
  H_NES_C* handle;

  handle=(H_NES_C*)this->handle;

  handle->Audio.Volume.Ctrl.Channel[0]=Volume;

  SetVolume(handle);

}

void H_NES_CPP::SetVolume_L(int Volume)noexcept{
  H_NES_C* handle;

  handle=(H_NES_C*)this->handle;

  handle->Audio.Volume.Ctrl.Channel[1]=Volume;

  SetVolume(handle);
}

void H_NES_CPP::SetVolume_R(int Volume)noexcept{
  H_NES_C* handle;

  handle=(H_NES_C*)this->handle;

  handle->Audio.Volume.Ctrl.Channel[2]=Volume;

  SetVolume(handle);
}

void H_NES_CPP::SetPulseLR_Diff(int PulseLR_Diff)noexcept{
  H_NES_C* handle;

  handle=(H_NES_C*)this->handle;

  handle->Audio.Lock->lock();

  NES_SetPulseLR_Diff(handle,PulseLR_Diff);

  handle->Audio.Lock->unlock();
}

void H_NES_CPP::SetTelevisionStandards(TVStandards tvStandards){
  H_NES_C* handle;
  Hsize TelevisionStandards;
  int r;

  handle=(H_NES_C*)this->handle;

  switch(tvStandards){
    case TVStandards::PAL:
      TelevisionStandards=vH_NES_TelevisionStandards_PAL;
      break;
    case TVStandards::Dendy:
      TelevisionStandards=vH_NES_TelevisionStandards_Dendy;
      break;
    case TVStandards::NTSC:
    default:
      TelevisionStandards=vH_NES_TelevisionStandards_NTSC;
      break;
  }
  

  r=H_NES_SetTelevisionStandards(handle->nes,TelevisionStandards);
  if(r!=vH_NES_Result_OK){
    throw H_NES_GetErrorString(r);
  }


  handle->Audio.Lock->lock();

  handle->Audio.InSampleRate=GetCPUFreq(tvStandards)/4;
  SetSampleRate(handle);

  handle->Audio.Lock->unlock();

  this->tvStandards=tvStandards;

}

void H_NES_CPP::SetVblankNMI_Delay(int VblankNMI_Delay)noexcept{
  H_NES_C* handle;
  
  handle=(H_NES_C*)this->handle;

  H_NES_SetVblankNMI_Delay(handle->nes,VblankNMI_Delay);
}

void H_NES_CPP::SetMMC3_CountDelay(int MMC3_CountDelay)noexcept{
  H_NES_C* handle;
  
  handle=(H_NES_C*)this->handle;

  H_NES_SetMMC3_CountDelay(handle->nes,MMC3_CountDelay);
}

void H_NES_CPP::Start(void* ROM){
  H_NES_C* handle;
  int r;
  
  handle=(H_NES_C*)this->handle;
  
  r=H_NES_LoadROM(handle->nes,ROM);
  if(r!=vH_NES_Result_OK){
    throw H_NES_GetErrorString(r);
  }

  handle->NES_ThreadHandle=CreateThread(nullptr,0,NES_Thread,handle,0,nullptr);

  H_NES_WaitUntilRun(handle->nes);

}

void H_NES_CPP::Reset()noexcept{
  H_NES_C* handle;

  handle=(H_NES_C*)this->handle;

  H_NES_Reset(handle->nes);
  
}

void H_NES_CPP::Stop()noexcept{
  H_NES_C* handle;

  handle=(H_NES_C*)this->handle;
  
  H_NES_Stop(handle->nes);
  H_NES_UnloadROM(handle->nes);

  if(handle->NES_ThreadHandle!=nullptr){
    CloseHandle(handle->NES_ThreadHandle);
    handle->NES_ThreadHandle=nullptr;
  }

}

int H_NES_CPP::GetProgressSize(){
  H_NES_C* handle;

  handle=(H_NES_C*)this->handle;
  
  return H_NES_GetProgressSize(handle->nes);
}

int H_NES_CPP::GetProgress(unsigned char** Progress){
  H_NES_C* handle;
  unsigned char* progress;
  int Length;
  int r;

  handle=(H_NES_C*)this->handle;

  Length=H_NES_GetProgressSize(handle->nes);
  if(Length<1){
    throw "错误的存档长度";
  }
  progress=new unsigned char[Length];
  if(progress==nullptr){
    throw "堆内存不足或错误的存档长度导致无法存档";
  }
  r=H_NES_GetProgress(handle->nes,progress);

  if(r!=vH_NES_Result_OK){
    throw H_NES_GetErrorString(r);
  }

  Progress[0]=progress;

  return Length;
}

void H_NES_CPP::LoadProgress(unsigned char* Progress){
  H_NES_C* handle;
  int r;

  handle=(H_NES_C*)this->handle;
  
  r=H_NES_LoadProgress(handle->nes,Progress);
  if(r!=vH_NES_Result_OK){
    throw H_NES_GetErrorString(r);
  }

}

bool H_NES_CPP::GetIsRun(){
  H_NES_C* handle;
  bool r;

  handle=(H_NES_C*)this->handle;
  
  handle->Status.Lock->lock();
  r=handle->Status.IsRun;
  handle->Status.Lock->unlock();

  return r;
}

int H_NES_CPP::GetCPUFreq(TVStandards tvstd)noexcept{
  double CPUFreq;

  switch(tvstd){
    case TVStandards::PAL:
      CPUFreq=26601712.5/16.0;
      break;
    case TVStandards::Dendy:
      CPUFreq=26601712.5/15.0;
      break;
    case TVStandards::NTSC:
    default:
      CPUFreq=236250000.0/11.0/12.0;
      break;
  }

  return (int)CPUFreq;
}

int H_NES_CPP::StandardControllerStatus::ButtonToPos(uint32_t Button){
  int r=0;

  if(Button==0){
    return -1;
  }

  for(r=0;r<32;r++){
    if((Button&(((uint32_t)0x1U)<<r))!=0){
      return r;
    }
  }

  return -1;
}

uint32_t H_NES_CPP::StandardControllerStatus::PosToButton(int Button_Pos){
  return ((uint32_t)0x1U)<<Button_Pos;
}




class H_MessageQueue:ByteStream{
  public:

    void Post(void* p);
    int TryPost(void* p);
    void Receive(void* p);
    int TryReceive(void* p);

    H_MessageQueue(int QueueLength,int Size);
    ~H_MessageQueue();
  private:
    std::mutex* ReadLock;
    std::mutex* WriteLock;
    int Size;
};

H_MessageQueue::H_MessageQueue(int QueueLength,int Size):ByteStream((QueueLength+1)*Size){
  this->Size=Size;
  this->ReadLock=new std::mutex();
  this->WriteLock=new std::mutex();
}

H_MessageQueue::~H_MessageQueue(){
  delete ReadLock;
  delete WriteLock;
}

void H_MessageQueue::Post(void* p){
  WriteLock->lock();
  Write((unsigned char*)p,Size);
  WriteLock->unlock();
}

int H_MessageQueue::TryPost(void* p){
  int r;
  WriteLock->lock();
  r=TryWrite((unsigned char*)p,Size);
  WriteLock->unlock();
  return r;
}

void H_MessageQueue::Receive(void* p){
  ReadLock->lock();
  Read((unsigned char*)p,Size);
  ReadLock->unlock();
}

int H_MessageQueue::TryReceive(void* p){
  int r;
  ReadLock->lock();
  r=TryRead((unsigned char*)p,Size);
  ReadLock->unlock();
  return r;
}




static void* CALLBACK new_MessageQueue(int QueueLength,int Size){
  return new H_MessageQueue(QueueLength+1,Size);
}
static void CALLBACK delete_MessageQueue(void* MessageQueue){
  delete (H_MessageQueue*)MessageQueue;
}
static int CALLBACK MessageSned(void* MessageQueue,void* p){
  ((H_MessageQueue*)MessageQueue)->Post(p);
  return 0;
}
static void CALLBACK MessageReceive(void* MessageQueue,void* p){
  ((H_MessageQueue*)MessageQueue)->Receive(p);
}
static int CALLBACK MessageTryReceive(void* MessageQueue,void* p){
  return ((H_MessageQueue*)MessageQueue)->TryReceive(p);
}
static void CALLBACK ThreadExit(int RetVal){
  ExitThread(RetVal);//这种方法似乎会带来隐患(尤其在调用涉及C++栈对象的情况下, 尽管H_NES会在结束线程前通过回调将创建的相关C++对象释放掉)

  //在C函数中抛出异常在Release版本中会导致程序崩溃
//  throw (int)RetVal;//使用抛出异常的方式 在H_NES线程(本实例就是NES_Thread())中捕获该异常后返回
}

//NES调用来输出一个扫描行的颜色索引
static void CALLBACK RendersLine(void* v,Hbyte* ColorIndexArray,unsigned int line){
  H_NES_C* handle;

  handle=(H_NES_C*)v;

  memcpy(&handle->ColorIndexArray[256*line],ColorIndexArray,256);

  if(line==239){
    
    handle->nes_cpp->FrameOutput(handle->ColorIndexArray);
  }

}

static void AudioConvertOutput(H_NES_C* nes_c,Huint32* Samples,int Length);

//NES调用来输出音频样本
static void CALLBACK AudioOutput(void* v,void* Samples,int Length){
  Huint32* sample;

  sample=(Huint32*)Samples;

  while(Length>=vAudioConvertOutput_MaxNumOfSample){
    AudioConvertOutput((H_NES_C*)v,sample,vAudioConvertOutput_MaxNumOfSample);

    sample=&sample[vAudioConvertOutput_MaxNumOfSample];
    Length-=vAudioConvertOutput_MaxNumOfSample;
  }
  if(Length>0){
    AudioConvertOutput((H_NES_C*)v,sample,Length);
  }

}

//NES调用来获取按键按下信息
static Huint16 CALLBACK GetStandardControllerStatus(void* v){
  H_NES_C* handle;
  uint32_t Status;
  Huint16 r;

  handle=(H_NES_C*)v;

  r=handle->LastStandardControllerStatus;

  Status=handle->nes_cpp->GetStandardControllerStatus();


  // if((Left&(0x0001U<<vButton_SR1_Pos))!=0U){
  //   r|=vH_NES_StandardControllerStatus_1P_A|vH_NES_StandardControllerStatus_1P_B;
  // }else{
    if((Status&H_NES_CPP::StandardControllerStatus::Pad1P_A)!=0U){
      r|=vH_NES_StandardControllerStatus_1P_A;
    }else if((Status&H_NES_CPP::StandardControllerStatus::Pad1P_AutoA)!=0U){
      r^=vH_NES_StandardControllerStatus_1P_A;
    }else{
      r&=(~vH_NES_StandardControllerStatus_1P_A);
    }
    if((Status&H_NES_CPP::StandardControllerStatus::Pad1P_B)!=0U){
      r|=vH_NES_StandardControllerStatus_1P_B;
    }else if((Status&H_NES_CPP::StandardControllerStatus::Pad1P_AutoB)!=0U){
      r^=vH_NES_StandardControllerStatus_1P_B;
    }else{
      r&=(~vH_NES_StandardControllerStatus_1P_B);
    }
  // }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad1P_Select)!=0U){
    r|=vH_NES_StandardControllerStatus_1P_Select;
  }else{
    r&=(~vH_NES_StandardControllerStatus_1P_Select);
  }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad1P_Start)!=0U){
    r|=vH_NES_StandardControllerStatus_1P_Start;
  }else{
    r&=(~vH_NES_StandardControllerStatus_1P_Start);
  }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad1P_Up)!=0U){
    r|=vH_NES_StandardControllerStatus_1P_Up;
  }else{
    r&=(~vH_NES_StandardControllerStatus_1P_Up);
  }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad1P_Down)!=0U){
    r|=vH_NES_StandardControllerStatus_1P_Down;
  }else{
    r&=(~vH_NES_StandardControllerStatus_1P_Down);
  }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad1P_Left)!=0U){
    r|=vH_NES_StandardControllerStatus_1P_Left;
  }else{
    r&=(~vH_NES_StandardControllerStatus_1P_Left);
  }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad1P_Right)!=0U){
    r|=vH_NES_StandardControllerStatus_1P_Right;
  }else{
    r&=(~vH_NES_StandardControllerStatus_1P_Right);
  }
  

  // if((Right&(0x0001U<<vButton_SR1_Pos))!=0U){
  //   r|=vH_NES_StandardControllerStatus_2P_A|vH_NES_StandardControllerStatus_2P_B;
  // }else{
    if((Status&H_NES_CPP::StandardControllerStatus::Pad2P_A)!=0U){
      r|=vH_NES_StandardControllerStatus_2P_A;
    }else if((Status&H_NES_CPP::StandardControllerStatus::Pad2P_AutoA)!=0U){
      r^=vH_NES_StandardControllerStatus_2P_A;
    }else{
      r&=(~vH_NES_StandardControllerStatus_2P_A);
    }
    if((Status&H_NES_CPP::StandardControllerStatus::Pad2P_B)!=0U){
      r|=vH_NES_StandardControllerStatus_2P_B;
    }else if((Status&H_NES_CPP::StandardControllerStatus::Pad2P_AutoB)!=0U){
      r^=vH_NES_StandardControllerStatus_2P_B;
    }else{
      r&=(~vH_NES_StandardControllerStatus_2P_B);
    }
  // }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad2P_Select)!=0U){
    r|=vH_NES_StandardControllerStatus_2P_Select;
  }else{
    r&=(~vH_NES_StandardControllerStatus_2P_Select);
  }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad2P_Start)!=0U){
    r|=vH_NES_StandardControllerStatus_2P_Start;
  }else{
    r&=(~vH_NES_StandardControllerStatus_2P_Start);
  }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad2P_Up)!=0U){
    r|=vH_NES_StandardControllerStatus_2P_Up;
  }else{
    r&=(~vH_NES_StandardControllerStatus_2P_Up);
  }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad2P_Down)!=0U){
    r|=vH_NES_StandardControllerStatus_2P_Down;
  }else{
    r&=(~vH_NES_StandardControllerStatus_2P_Down);
  }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad2P_Left)!=0U){
    r|=vH_NES_StandardControllerStatus_2P_Left;
  }else{
    r&=(~vH_NES_StandardControllerStatus_2P_Left);
  }
  if((Status&H_NES_CPP::StandardControllerStatus::Pad2P_Right)!=0U){
    r|=vH_NES_StandardControllerStatus_2P_Right;
  }else{
    r&=(~vH_NES_StandardControllerStatus_2P_Right);
  }

  


  handle->LastStandardControllerStatus=r;

  return r;
}

static void GetLL_Function(H_NES_LL_Function* LL_Function){

  LL_Function->Thread.new_MessageQueue=new_MessageQueue;
  LL_Function->Thread.delete_MessageQueue=delete_MessageQueue;
  LL_Function->Thread.MessageSned=MessageSned;
  LL_Function->Thread.MessageReceive=MessageReceive;
  LL_Function->Thread.MessageTryReceive=MessageTryReceive;
  LL_Function->Thread.ThreadExit=ThreadExit;
  
  LL_Function->Console.RendersLine=RendersLine;
  LL_Function->Console.AudioOutput=AudioOutput;
  LL_Function->Console.GetStandardControllerStatus=GetStandardControllerStatus;

}

#define vNES_APU_Shield_Pulse1                                    0
#define vNES_APU_Shield_Pulse2                                    0
#define vNES_APU_Shield_Triangle                                  0
#define vNES_APU_Shield_Noise                                     0
#define vNES_APU_Shield_DMC                                       0
#define vNES_APU_Shield_Ext                                       0

static void AudioConvertOutput(H_NES_C* nes_c,Huint32* Samples,int Length){

  //32bit(每个样本) Bit[0:3]:Pulse1 Bit[4:7]:Pulse2 Bit[8:11]:Triangle Bit[12:15]:Noise Bit[16:22]:DMC Bit[24:31]:外部通道

  /*
    output = pulse_out + tnd_out

                              95.88                        95.88*(pulse1 + pulse2)
    pulse_out = ------------------------------------  =  ----------------------------
                (8128 / (pulse1 + pulse2)) + 100          8128+100*(pulse1 + pulse2)

                                          159.79                                   159.79*( (triangle / 8227) + (noise / 12241) + (dmc / 22638) )
    tnd_out = -------------------------------------------------------------  =   -----------------------------------------------------------------
                                        1                                                           
              ----------------------------------------------------- + 100         1 + 100*( (triangle / 8227) + (noise / 12241) + (dmc / 22638) )
                (triangle / 8227) + (noise / 12241) + (dmc / 22638)
  */

  H_AudioResample_InputType* AudioBuffer;
  int i;
  Huint32 Buffer;
  H_AudioResample_InputType PulseLR_Diff;

  struct{
    H_AudioResample_InputType K;
    H_AudioResample_InputType LY1;
    H_AudioResample_InputType RY1;
  }LPF;
  struct{
    H_AudioResample_InputType K;
    H_AudioResample_InputType LY1;
    H_AudioResample_InputType LX1;
    H_AudioResample_InputType RY1;
    H_AudioResample_InputType RX1;
  }HPF;

  H_AudioResample_InputType L;
  H_AudioResample_InputType R;


  nes_c->Audio.Lock->lock();


  AudioBuffer=nes_c->Audio.AudioConvertOutputBuffer;

  PulseLR_Diff=nes_c->Audio.PulseLR_Diff;
  LPF.K=nes_c->Audio.LPF.K;
  LPF.LY1=nes_c->Audio.LPF.LY1;
  LPF.RY1=nes_c->Audio.LPF.RY1;
  HPF.K=nes_c->Audio.HPF.K;
  HPF.LY1=nes_c->Audio.HPF.LY1;
  HPF.LX1=nes_c->Audio.HPF.LX1;
  HPF.RY1=nes_c->Audio.HPF.RY1;
  HPF.RX1=nes_c->Audio.HPF.RX1;


  if(PulseLR_Diff<cNES_Audio_FloatConst(0.0)){
    H_AudioResample_InputType pulse1;
    H_AudioResample_InputType pulse2;
    H_AudioResample_InputType triangle;
    H_AudioResample_InputType noise;
    H_AudioResample_InputType dmc;
    H_AudioResample_InputType tnd_out;
    H_AudioResample_InputType t_inputtype;

    PulseLR_Diff=cNES_Audio_FloatConst(1.0)+PulseLR_Diff;// (- (- PulseLR_Diff)) == + PulseLR_Diff
    
    for(i=0;i<Length;i++){

      Buffer=((Huint32*)Samples)[i];

#if vNES_APU_Shield_Pulse1 == 0
      pulse1=(H_AudioResample_InputType)(Buffer&0x0000000FU);
#else
      pulse1=cNES_Audio_FloatConst(0.0);
#endif
#if vNES_APU_Shield_Pulse2 == 0
      pulse2=(H_AudioResample_InputType)((Buffer>>4)&0x0000000FU);
#else
      pulse2=cNES_Audio_FloatConst(0.0);
#endif
      L=pulse1*PulseLR_Diff+pulse2;
      R=pulse1+pulse2*PulseLR_Diff;

      L=cNES_Audio_FloatConst(2147483647.0)*cNES_Audio_FloatConst(95.88)*L/((cNES_Audio_FloatConst(8128.0) + cNES_Audio_FloatConst(100.0)*L));
      R=cNES_Audio_FloatConst(2147483647.0)*cNES_Audio_FloatConst(95.88)*R/((cNES_Audio_FloatConst(8128.0) + cNES_Audio_FloatConst(100.0)*R));

#if vNES_APU_Shield_Triangle == 0
      triangle=(H_AudioResample_InputType)((Buffer>>8)&0x0000000FU);
#else
      triangle=cNES_Audio_FloatConst(0.0);
#endif
#if vNES_APU_Shield_Noise == 0
      noise=(H_AudioResample_InputType)((Buffer>>12)&0x0000000FU);
#else
      noise=cNES_Audio_FloatConst(0.0);
#endif
#if vNES_APU_Shield_DMC == 0
      dmc=(H_AudioResample_InputType)(Buffer>>16);
#else
      dmc=cNES_Audio_FloatConst(0.0);
#endif
      tnd_out=triangle/cNES_Audio_FloatConst(8227.0) + noise/cNES_Audio_FloatConst(12241.0) + dmc/cNES_Audio_FloatConst(22638.0);
      tnd_out=cNES_Audio_FloatConst(2147483647.0)*cNES_Audio_FloatConst(159.79)*tnd_out/(cNES_Audio_FloatConst(1.0) + cNES_Audio_FloatConst(100.0)*tnd_out);
      
      L+=tnd_out;
      R+=tnd_out;


      //Y(n) = Y(n-1)*(1-k) + X(n)*k = Y(n-1)+(X(n)-Y(n-1))*k
      L=LPF.LY1 + (L-LPF.LY1)*LPF.K;
      LPF.LY1=L;

      R=LPF.RY1 + (R-LPF.RY1)*LPF.K;
      LPF.RY1=R;


      //Y(n) = Y(n-1)*k + X(n)*k - X(n-1)*k = ( Y(n-1) + X(n) - X(n-1) )*k
      t_inputtype=L;
      L=(HPF.LY1+L-HPF.LX1)*HPF.K;
      HPF.LY1=L;
      HPF.LX1=t_inputtype;

      t_inputtype=R;
      R=(HPF.RY1+R-HPF.RX1)*HPF.K;
      HPF.RX1=t_inputtype;
      HPF.RY1=R;


      AudioBuffer[0]=L;
      AudioBuffer[1]=R;
      AudioBuffer=&AudioBuffer[2];
    }

  }else{
    H_AudioResample_InputType pulse1;
    H_AudioResample_InputType pulse2;
    H_AudioResample_InputType triangle;
    H_AudioResample_InputType noise;
    H_AudioResample_InputType dmc;
    H_AudioResample_InputType tnd_out;
    H_AudioResample_InputType t_inputtype;

    PulseLR_Diff=cNES_Audio_FloatConst(1.0)-PulseLR_Diff;

    for(i=0;i<Length;i++){

      Buffer=((Huint32*)Samples)[i];

#if vNES_APU_Shield_Pulse1 == 0
      pulse1=(H_AudioResample_InputType)(Buffer&0x0000000FU);
#else
      pulse1=cNES_Audio_FloatConst(0.0);
#endif
#if vNES_APU_Shield_Pulse2 == 0
      pulse2=(H_AudioResample_InputType)((Buffer>>4)&0x0000000FU);
#else
      pulse2=cNES_Audio_FloatConst(0.0);
#endif
      L=pulse1+pulse2*PulseLR_Diff;
      R=pulse1*PulseLR_Diff+pulse2;

      L=cNES_Audio_FloatConst(2147483647.0)*cNES_Audio_FloatConst(95.88)*L/((cNES_Audio_FloatConst(8128.0) + cNES_Audio_FloatConst(100.0)*L));
      R=cNES_Audio_FloatConst(2147483647.0)*cNES_Audio_FloatConst(95.88)*R/((cNES_Audio_FloatConst(8128.0) + cNES_Audio_FloatConst(100.0)*R));

#if vNES_APU_Shield_Triangle == 0
      triangle=(H_AudioResample_InputType)((Buffer>>8)&0x0000000FU);
#else
      triangle=cNES_Audio_FloatConst(0.0);
#endif
#if vNES_APU_Shield_Noise == 0
      noise=(H_AudioResample_InputType)((Buffer>>12)&0x0000000FU);
#else
      noise=cNES_Audio_FloatConst(0.0);
#endif
#if vNES_APU_Shield_DMC == 0
      dmc=(H_AudioResample_InputType)(Buffer>>16);
#else
      dmc=cNES_Audio_FloatConst(0.0);
#endif
      tnd_out=triangle/cNES_Audio_FloatConst(8227.0) + noise/cNES_Audio_FloatConst(12241.0) + dmc/cNES_Audio_FloatConst(22638.0);
      tnd_out=cNES_Audio_FloatConst(2147483647.0)*cNES_Audio_FloatConst(159.79)*tnd_out/(cNES_Audio_FloatConst(1.0) + cNES_Audio_FloatConst(100.0)*tnd_out);
      
      L+=tnd_out;
      R+=tnd_out;


      //Y(n) = Y(n-1)*(1-k) + X(n)*k = Y(n-1)+(X(n)-Y(n-1))*k
      L=LPF.LY1 + (L-LPF.LY1)*LPF.K;
      LPF.LY1=L;

      R=LPF.RY1 + (R-LPF.RY1)*LPF.K;
      LPF.RY1=R;


      //Y(n) = Y(n-1)*k + X(n)*k - X(n-1)*k = ( Y(n-1) + X(n) - X(n-1) )*k
      t_inputtype=L;
      L=(HPF.LY1+L-HPF.LX1)*HPF.K;
      HPF.LY1=L;
      HPF.LX1=t_inputtype;

      t_inputtype=R;
      R=(HPF.RY1+R-HPF.RX1)*HPF.K;
      HPF.RX1=t_inputtype;
      HPF.RY1=R;


      AudioBuffer[0]=L;
      AudioBuffer[1]=R;
      AudioBuffer=&AudioBuffer[2];
    }

  }

  nes_c->Audio.LPF.K=LPF.K;
  nes_c->Audio.LPF.LY1=LPF.LY1;
  nes_c->Audio.LPF.RY1=LPF.RY1;
  nes_c->Audio.HPF.K=HPF.K;
  nes_c->Audio.HPF.LY1=HPF.LY1;
  nes_c->Audio.HPF.LX1=HPF.LX1;
  nes_c->Audio.HPF.RY1=HPF.RY1;
  nes_c->Audio.HPF.RX1=HPF.RX1;


  

  H_AudioResample_OutputType* output;

  while(0!=H_AudioResample_Input(nes_c->Audio.Resampler,nes_c->Audio.AudioConvertOutputBuffer,Length*2)){
    
    output=H_AudioResample_Output(nes_c->Audio.Resampler);
    while(output!=NULL){
      nes_c->nes_cpp->AudioOutput(output,nes_c->Audio.SizeOfPerOutput/2);
      output=H_AudioResample_Output(nes_c->Audio.Resampler);
    }
  }

  output=H_AudioResample_Output(nes_c->Audio.Resampler);
  while(output!=NULL){
    nes_c->nes_cpp->AudioOutput(output,nes_c->Audio.SizeOfPerOutput/2);
    output=H_AudioResample_Output(nes_c->Audio.Resampler);
  }
  nes_c->Audio.Lock->unlock();

}




